/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

void slam3D_ini(double *L,double *R,double *x,double *icb,double *g,double *p,double *X0,double *Xpos,double *Xpri,int *ind,int *indf,double *z,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,double lambda,double *ssd,double *er,double th);
void slam3D_iter_mod1(double *L,double *R,double *x,double *icb,double *g,double *p,double *X0,double *Xpos,double *Xpri,int *ind,int *indf,double *z,double *W,double *P,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,double lambda,double *ssd,double *er,double th);
void slam3D_iter_map(double *L,double *R,double *x,double *icb,double *g,double *p,double *X0,double *Xpos,double *Xpri,double *map,int *ind,int *indf,double *z,int *indm,int *indfm,double *zm,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int Nm,int num_frames,double lambda,double *ssd,double *er,double th);
void calc_Ces(double *Ce,double *C,double *e,double P,int ind,int num_frames);
void calc_Ce(double *Ce,double *C,double *e,double P, int ind,int frame,int num_frames);
void calc_CCs(double *xx,double *C,double P,int p,int ind);
void calc_CC(double *vv,double *vx,double *xx,double *C,double P,int Nv,int Nx,int p,int ind,int frame);
void calc_CCs3(double *xx,double *C,double P,int ind);
void calc_CC3(double *vv,double *vx,double *xx,double *C,double P,int Nv,int Nx,int ind,int frame);
void proj3D_funs(double *e,double *J,double *q,double *t,double *p,double *pts,double *X,int ind,double th);
void proj3D_fun(double *e,double *J,double *q,double *t,double *p,double *pts,double *X,int ind,double th);
void proj3D_funs1(double *e,double *J,double *q,double *t,double *p,double *pts,double *X,int ind,double th);
void proj3D_fun1(double *e,double *J,double *q,double *t,double *p,double *pts,double *X,int ind,double th);
void stereo_proj3D_fun(double *e,double *J, double *q,double *t,double *p,double *R,double *t0,double *pts,double *X,int ind,double th);
void slam3D_batcht(double *L,double *R,double *x,double *icb,double *g,double *p,double **Xout,double *X0,double *Xpri,int *ind,int *indf,double *z,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,double lambda,double *er,double th,int cyc);
void slam3D_batch(double *L,double *R,double *x,double *icb,double *g,double *p,double **Xout,double *X0,double *Xpri,int *ind,int *indf,double *z,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,int start_frame,double lambda,double *er,double th,int cyc);
void slam3D_batch1(double *L,double *R,double *x,double *icb,double *g,double *p,double **Xout,double *X0,double *Xpri,int *ind,int *indf,double *z,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,int start_frame,double lambda,double *er,double th,int cyc);
void slam3D_batch_map(double *L,double *R,double *x,double *icb,double *g,double *p,double **Xout,double *X0,double *Xpri,double *map,int *ind,int *indf,double *z,int *indm,int *indfm,double *zm,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int Nm,int num_frames,double lambda,double *er,double th,int cyc);
void slam3D_batch_mod0(double *L,double *R,double *x,double *icb,double *g,double *p,double **Xout,double *X0,double *Xpri,int *ind,int *indf,double *z,double *u,double *W,double *P,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,int start_frame,double lambda,double *er,double th,int cyc);
void slam3D_batch_mod_s(double *L,double *R,double *x,double *icb,double *g,double *p,double **Xout,double *X0,double *Xpri,int *ind,int *indf,double *z,double *u,double *W,double *P,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,int start_frame,double lambda,double *er,double th,int cyc);
/** calculates bundle adjustment for for poses and landmark coordinates for single camera with traveled distance odometry
* @param *L [in] (D-(num_frames-1)x7)x3 helper array 
* @param *R [in] (D-(num_frames-1)x7)x3 (num_frames-1)x7x(num_frames-1)x7 helper array helper array 
* @param *x [in] (D-(num_frames-1)x7)x1 helper array 
* @param *icb [in] (num_frames-1)x7x(D-(num_frames-1)x7) helper array 
* @param *g [in] Dx1 helper array 
* @param *p [in]  intrinsic camera parameters . Focal length and image center represented in 3 element array 
* @param **Xout [out] pointer to slam estimates 
* @param *X0 [in] initial poses 
* @param *Xpri [in] Dx3 array initial slam estimate  
* @param *ind [in] 1xN array, ith element tells from which landmark it measurement is
* @param *indf [in] 1xN array , ith element tells from which pose it measurement is, negative ith value corresponds to frozen poses
* @param *z [in] 2xN array of landmark measurements
* @param *u[in]  traveled distance odometry 
* @param *W [in] odometry covariance
* @param *P [in] measurement covariance 
* @param *vv [in] (num_frames-1)x7x(num_frames-1)x7 helper array 
* @param *vx [in] (num_frames-1)x7x(D-(num_frames-1)x7) helper array 
* @param *xx [in] (D-(num_frames-1)x7)x3 helper array 
* @param *Ce [in] Dx1 helper array 
* @param *C [in] helper array 
* @param *D [in]  number of estimated parameters
* @param *N [in]  number of landmark measurements
* @param *num_frames [in]  number+1 of poses to be estimated 
* @param *start_frame [in]  which pose in X0 is first 
* @param *lambda [in]  lambda in levenber-marquardt
* @param *er [in] helper array 
* @param th [in] outlier threshold
* @param cyc [in] number of optimisation iterations
*/

void slam3D_batch_mod1(double *L,double *R,double *x,double *icb,double *g,double *p,double **Xout,double *X0,double *Xpri,int *ind,int *indf,double *z,double *W,double *P,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,double lambda,double *er,double th,int cyc);
void slam3D_stereo_batch(double *L,double *R,double *x,double *icb,double *g,double *pl,double *pr,double *stereo_par,double **Xout,double *X0,double *Xpri,int *ind,int *indf,int *indc,double *z,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,int start_frame,double lambda,double *er,double th,int cyc);
/** calculates bundle adjustment for for poses and landmark coordinates for stereo camera
* @param *L [in] (D-(num_frames-1)x7)x3 helper array 
* @param *R [in] (D-(num_frames-1)x7)x3 (num_frames-1)x7x(num_frames-1)x7 helper array helper array 
* @param *x [in] (D-(num_frames-1)x7)x1 helper array 
* @param *icb [in] (num_frames-1)x7x(D-(num_frames-1)x7) helper array 
* @param *g [in] Dx1 helper array 
* @param *pl [in]  intrinsic camera parameters for camera 1. Focal length and image center represented in 3 element array 
* @param *pr [in]  intrinsic camera parameters for camera 2. Focal length and image center represented in 3 element array 
* @param *stereo_par [in]  7x1 array relative stereo pose between cameras, quaternion and 3D-translation 
* @param **Xout [out] pointer to slam estimates 
* @param *X0 [in] initial poses 
* @param *Xpri [in] Dx3 array initial slam estimate  
* @param *ind [in] 1xN array, ith element tells from which landmark it measurement is
* @param *indf [in] 1xN array , ith element tells from which pose it measurement is, negative ith value corresponds to frozen poses
* @param *indc [in] 1xN array, ith element tells from which camera it measurement is
* @param *z [in] 2xN array of landmark measurements
* @param *vv [in] (num_frames-1)x7x(num_frames-1)x7 helper array 
* @param *vx [in] (num_frames-1)x7x(D-(num_frames-1)x7) helper array 
* @param *xx [in] (D-(num_frames-1)x7)x3 helper array 
* @param *Ce [in] Dx1 helper array 
* @param *C [in] helper array 
* @param *D [in]  number of estimated parameters
* @param *N [in]  number of landmark measurements
* @param *num_frames [in]  number+1 of poses to be estimated 
* @param *start_frame [in]  which pose in X0 is first 
* @param *lambda [in]  lambda in levenber-marquardt
* @param *er [in] helper array 
* @param th [in] outlier threshold
* @param cyc [in] number of optimisation iterations
*/
void slam3D_iter_stereo_batch(double *L,double *R,double *x,double *icb,double *g,double *pl,double *pr,double *stereo_par,double *X0,double *Xpos,double *Xpri,int *ind,int *indf,int *indc,double *z,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,int start_frame,double lambda,double *ssd,double *er,double th);
void slam3D_struc_stereo_batch(double *L,double *g,double *pl,double *pr,double *stereo_par,double **Xout,double *X0,double *Xpri,int *ind,int *indc,double *z,double *xx,double *Ce,double *C,int D,int N,double lambda,double *er,double th,int cyc);
void motion3Db3(double *e,double *J,double *X,double *w,int j,int i);
void motion3Da3(double *e,double *J,double *X,double *w,int j,int i);
void calc_3D_link(double *e,double *J,double *X,double *t,int j,int i);
void slam3D_stereo_er(double *pl,double *pr,double *stereo_par,double *X0,double *Xpri,int *ind,int *indf,int *indc,double *z,int D,int N,int num_frames,int start_frame,double *ssd,double *er,double *depth);
void slam3D_batch_er(double *p,double *X0,double *Xpri,int *ind,int *indf,double *z,int D,int N,int num_frames,int start_frame,double *ssd,double *er,double *depth);
void slam3D_batch_er1(double *p,double *X0,double *Xpri,int *ind,int *indf,double *z,int D,int N,int num_frames,int start_frame,double *ssd,double *er,double *depth);
void slam3D_iter_mod_s(double *L,double *R,double *x,double *icb,double *g,double *pl,double *X0,double *Xpos,double *Xpri,int *ind,int *indf,double *z,double *u,double *W,double *P,double *vv,double *vx,double *xx,double *Ce,double *C,int D,int N,int num_frames,int start_frame,double lambda,double *ssd,double *er,double th);
void calc_3D_link0(double *e,double *J,double *X0,double *X,double *t,int j);
void calc_3D_link(double *e,double *J,double *X,double *t,int j,int i);
